#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <string.h>
#include <unistd.h>
#include "iot_watchdog.h"
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "iot_gpio_ex.h"
#include "hi_timer.h"
#include "robot_motor.h"
#include "robot_encoder.h"

// DEBUG模式
//#define DEBUG

#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

#define IOT_PWM_PORT_PWM0 0
#define IOT_PWM_PORT_PWM1 1
#define IOT_PWM_PORT_PWM3 3
#define IOT_PWM_PORT_PWM4 4

#define ONE_SECOND    (1000)
#define PWM_FREQUENCY 32000 /*一般设置为16kHz*/

#define Kp 250      // 比例系数
#define Ki 25       // 积分系数
#define Kd 0        // 微分系数

#define INTEGRAL_SEPARATE_THRESHOLD 0.01

/* 定时器信号量 */
int timerSignal = 0;

/* 计数器 */
int encoderLeftCounter = 0;
int encoderRightCounter = 0;

IotGpioValue value13 = IOT_GPIO_VALUE0;
IotGpioValue value14 = IOT_GPIO_VALUE0;
/* 参数 */
float diameter = 0.065; // 直径(m)
float resolution = 528; // 编码器分辨率(线数)
float Ts = 0.04;           // 采样周期(s)

/* 速度 */
float currentLeftSpeed = 0;
float currentRightSpeed = 0;

/* PID结构体 */
MotorPID pid_right_motor, pid_left_motor;

/* 设置速度 */
float leftSpeedSet = 0;   // 单位:m/s
float rightSpeedSet = 0;  // 单位:m/s

/* 脉冲数换算速度 */
float count2speed(int count) {
    return M_PI*count*diameter/(resolution*Ts);
}

/* 占空比大致换算速度 */
float pwmduty2speed(int duty){
    float maxSpeed = 0.24;
    return maxSpeed/100*duty;
}

/* 设置电机速度(百分比) */

void setLeftMotorDuty(int duty) {
    leftSpeedSet = pwmduty2speed(duty);
}

void setRightMotorDuty(int duty) {
    rightSpeedSet = pwmduty2speed(duty);
}

/* 设置电机速度(线速度) */

void setLeftMotorSpeed(float speed) {
    leftSpeedSet = speed;
}

void setRightMotorSpeed(float speed) {
    rightSpeedSet = speed;
}


void PIDinit(void){
    /* 右电机 */
    pid_right_motor.SetSpeed=0;
    pid_right_motor.ActualSpeed=0;
    pid_right_motor.err=0;
    pid_right_motor.err_last=0;
    pid_right_motor.integral=0;
    pid_right_motor.voltage=0;


    /* 左电机 */
    pid_left_motor.SetSpeed=0;
    pid_left_motor.ActualSpeed=0;
    pid_left_motor.err=0;
    pid_left_motor.err_last=0;
    pid_left_motor.integral=0;
    pid_left_motor.voltage=0;

    printf("[LOG] PID initialize succeed.\r\n");
}

/* 位置式pid控制 */

void PID_control_right(float setspeed, float actualspeed) {
    pid_right_motor.SetSpeed = setspeed;
    pid_right_motor.ActualSpeed = actualspeed;
    pid_right_motor.err = pid_right_motor.SetSpeed-pid_right_motor.ActualSpeed;
    
    int index = 0;  // 积分分离
    if (pid_right_motor.voltage == 99){
        if (fabs(pid_right_motor.err) > INTEGRAL_SEPARATE_THRESHOLD){
            index = 0;
        }
        else{
            index = 1;
            if (pid_right_motor.err < 0){
                pid_right_motor.integral += pid_right_motor.err; // 抗饱和
            }
        }
    }
    else if (pid_right_motor.voltage == -99){
        if (fabs(pid_right_motor.err) > INTEGRAL_SEPARATE_THRESHOLD){
            index = 0;
        }
        else{
            index = 1;
            if (pid_right_motor.err > 0){
                pid_right_motor.integral += pid_right_motor.err; // 抗饱和
            }
        }
    }
    
    int incrementVolt = Kp*pid_right_motor.err + Ki * index * pid_right_motor.integral + Kd*(pid_right_motor.err - pid_right_motor.err_last); // 计算增量
    pid_right_motor.voltage += incrementVolt; // 增量式
    // 上下限
    pid_right_motor.voltage = (pid_right_motor.voltage > 99)? 99:pid_right_motor.voltage; 
    pid_right_motor.voltage = (pid_right_motor.voltage < -99)? -99:pid_right_motor.voltage; 
    
    pid_right_motor.err_last = pid_right_motor.err;
#ifdef DEBUG
    printf("[DEBUG] right setspeed:%.4f PID err: %.4f incre: %d duty:%d\r\n", setspeed , pid_right_motor.err, incrementVolt, pid_right_motor.voltage);
#endif
    // 设置速度
    int speed = pid_right_motor.voltage;
    IoTPwmStop(IOT_PWM_PORT_PWM3);
    IoTPwmStop(IOT_PWM_PORT_PWM4);
    if (speed > 0){
        IoTPwmStart(IOT_PWM_PORT_PWM3, 0, PWM_FREQUENCY);   //GPIO0
        IoTPwmStart(IOT_PWM_PORT_PWM4, speed, PWM_FREQUENCY); //GPIO1
    }
    else{
        IoTPwmStart(IOT_PWM_PORT_PWM3, -speed, PWM_FREQUENCY);   //GPIO0
        IoTPwmStart(IOT_PWM_PORT_PWM4, 0, PWM_FREQUENCY); //GPIO1
    }
}

void PID_control_left(float setspeed, float actualspeed) {
    pid_left_motor.SetSpeed = setspeed;
    pid_left_motor.ActualSpeed = actualspeed;
    pid_left_motor.err = pid_left_motor.SetSpeed-pid_left_motor.ActualSpeed;
    int index = 0;  // 积分分离
    if (pid_left_motor.voltage == 99){
        if (fabs(pid_left_motor.err) > INTEGRAL_SEPARATE_THRESHOLD){
            index = 0;
        }
        else{
            index = 1;
            if (pid_left_motor.err < 0){
                pid_left_motor.integral += pid_left_motor.err; // 抗饱和
            }
        }
    }
    else if (pid_left_motor.voltage == -99){
        if (fabs(pid_left_motor.err) > INTEGRAL_SEPARATE_THRESHOLD){
            index = 0;
        }
        else{
            index = 1;
            if (pid_left_motor.err > 0){
                pid_left_motor.integral += pid_left_motor.err; // 抗饱和
            }
        }
    }
    
    int incrementVolt = Kp*pid_left_motor.err + Ki * index * pid_left_motor.integral + Kd*(pid_left_motor.err - pid_left_motor.err_last); // 计算增量
    pid_left_motor.voltage += incrementVolt; // 增量式
    pid_left_motor.err_last = pid_left_motor.err;
    // 上下限
    pid_left_motor.voltage = (pid_left_motor.voltage > 99)? 99:pid_left_motor.voltage; 
    pid_left_motor.voltage = (pid_left_motor.voltage < -99)? -99:pid_left_motor.voltage; 
#ifdef DEBUG
    printf("[DEBUG] left setspeed:%.4f PID err: %.4f incre: %d duty:%d\r\n", setspeed , pid_left_motor.err, incrementVolt, pid_left_motor.voltage);
#endif
    int speed = pid_left_motor.voltage;
    IoTPwmStop(IOT_PWM_PORT_PWM0);
    IoTPwmStop(IOT_PWM_PORT_PWM1);
    if (speed > 0){
        IoTPwmStart(IOT_PWM_PORT_PWM0, 0, PWM_FREQUENCY);   //GPIO9 
        IoTPwmStart(IOT_PWM_PORT_PWM1, speed, PWM_FREQUENCY);   //GPIO10
    }
    else{
        IoTPwmStart(IOT_PWM_PORT_PWM0, -speed, PWM_FREQUENCY);   //GPIO9 
        IoTPwmStart(IOT_PWM_PORT_PWM1, 0, PWM_FREQUENCY);   //GPIO10
    }
}


/* 定时器1 回调函数 */
void Timer1Callback(void *arg)
{
    (void)arg;
    /* 求取速度值 */
    currentLeftSpeed = count2speed(encoderLeftCounter);
    currentRightSpeed = count2speed(encoderRightCounter);
#ifdef DEBUG
    printf("[TIMER] Timer1 Callback. Left speed:%.4f m/s Right speed:%.4f m/s\r\n",currentLeftSpeed, currentRightSpeed);
#endif
    /* 将信号量置1 */    
    timerSignal = (timerSignal == 0)? 1: 0;

    encoderLeftCounter = 0;
    encoderRightCounter = 0;
}

/* 左电机中断处理函数 */
static hi_void leftCounterHandler(char *arg) {
    IoTGpioGetInputVal(IOT_IO_NAME_GPIO_13, &value13);
    if (value13 == IOT_GPIO_VALUE1) {
        encoderLeftCounter--;   // 反转
    }
    else{
        encoderLeftCounter++;   // 正传
    }
}

/* 右电机中断处理函数 */
static hi_void rightCounterHandler(char *arg) {
    IoTGpioGetInputVal(IOT_IO_NAME_GPIO_14, &value14);
    if (value14 == IOT_GPIO_VALUE0) {    // 与左电机规则相反
        encoderRightCounter--;   // 反转
    }
    else{
        encoderRightCounter++;   // 正传
    }
}

/* 定时器创建 */
static hi_void *EncoderTaskEntry(const char *arg) {
    
    /* 初始化PID */
    PIDinit();

    unsigned int timerId1;
    unsigned int ret;
    /* 新建定时器 */

    ret = hi_timer_create(&timerId1);
    if (ret != HI_ERR_SUCCESS) {
        printf("[ERROR] Failed to create timer 1\r\n");
    }
    else {
        printf("[LOG] Timer 1 created succeed.\r\n");

        /* 启动定时器 */

        ret = hi_timer_start(timerId1, HI_TIMER_TYPE_PERIOD, Ts*ONE_SECOND, Timer1Callback, 0); 
        if (ret != HI_ERR_SUCCESS) {
            printf("Failed to start timer 1\r\n");
        }
        else {
            printf("[LOG] Timer 1 started succeed.\r\n");
        }
    }

    /* gpio初始化：复用为gpio、设置为输入，上拉  */

    /* 传感器1 A GPIO11 */
    IoSetFunc(11, 0);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_11, IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_11, IOT_IO_PULL_UP);

    /* 传感器2 A GPIO12 */
    IoSetFunc(12, 0);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_12, IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_12, IOT_IO_PULL_UP);

    /* 传感器1 B GPIO13 */
    IoSetFunc(13, 4);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_13, IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_13, IOT_IO_PULL_UP);

    /* 传感器2 B GPIO14 */
    IoSetFunc(14, 4);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_14, IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_14, IOT_IO_PULL_UP);

    /* 通过上升沿触发中断计数 */
    IoTGpioRegisterIsrFunc(IOT_IO_NAME_GPIO_12, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_RISE_LEVEL_HIGH, rightCounterHandler, NULL); // 上升沿触发中断
    IoTGpioRegisterIsrFunc(IOT_IO_NAME_GPIO_11, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_RISE_LEVEL_HIGH, leftCounterHandler, NULL); // 上升沿触发中断
}

/* PID控制处理函数 */
void PID_control_handler(){
    if (timerSignal){
        PID_control_left(leftSpeedSet,currentLeftSpeed);
        PID_control_right(rightSpeedSet,currentRightSpeed);
        timerSignal = 0;
    }
}

#define ENCODER_TASK_STACKSIZE  0x3000
#define ENCODER_TASK_PRIOR 28
#define ENCODER_TASK_NAME "ENCODERTASK"

void encoderTaskInit(void)
{
    osThreadAttr_t attr = {0};
    attr.name = ENCODER_TASK_NAME;
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_size = ENCODER_TASK_STACKSIZE;
    attr.priority = ENCODER_TASK_PRIOR;

    if (osThreadNew((osThreadFunc_t)EncoderTaskEntry, NULL, &attr) == NULL) {
        printf("[ERROR] Falied to create encoder task!\n");
    }
}

APP_FEATURE_INIT(encoderTaskInit);